#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <getopt.h>
#include <fcntl.h>
#include <time.h>
#include <sys/ioctl.h>
#include <linux/ioctl.h>
#include <sys/stat.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>

struct acc_measure{
	signed short x;
	signed short y;
	signed short z;
};

struct gyro_measure{
	signed short x;
	signed short y;
	signed short z;
};

#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))

static unsigned char mpu6500_regs[]=
{
	0x75 | 0x80, 0x70,		//	117	read id: 0x70
	
	0x3b | 0x80, 0x00,		//	accelerometer measurements	59:Xout_h
	0x3c | 0x80, 0x00,		//	...							60:Xout_l
	0x3d | 0x80, 0x00,		//	...							61:Yout_h
	0x3e | 0x80, 0x00,		//	...							62:Yout_l
	0x3f | 0x80, 0x00,		//	...							63:Zout_h
	0x40 | 0x80, 0x00,		//	...							64:Zout_l

	0x41 | 0x80, 0x00,		//	temperature measurements	65:Tout_h
	0x42 | 0x80, 0x00,		//	...							66:Tout_l

	0x43 | 0x80, 0x00,		//	gyroscope measurements	67:Xout_h
	0x44 | 0x80, 0x00,		//	...						68:Xout_l
	0x45 | 0x80, 0x00,		//	...						69:Yout_h
	0x46 | 0x80, 0x00,		//	...						70:Yout_l
	0x47 | 0x80, 0x00,		//	...						71:Zout_h
	0x48 | 0x80, 0x00,		//	...						72:Zout_l
};

static void acc_read(int fd);
static void temp_read(int fd);
static void gyro_read(int fd);
static void chip_id(int fd);
static void transfer(int fd, uint8_t *tx, uint8_t *rx, size_t len);

int main(int argc, char *argv[])
{
	int fd;
	unsigned char val[2];

	if (argc != 2)
	{
		printf("usage: ./app_mpu6500 /dev/mpu6500.0.1\n");
		exit(-1);
	}

	fd = open(argv[1], O_RDWR);
	if (fd < 0)
	{
		printf("can't open device");
		exit(-1);
	}

	//chip_id(fd);

	while(1)
	{
		acc_read(fd);
		gyro_read(fd);
		temp_read(fd);
		usleep(200000);
		printf("\n");
	}

	close(fd);
	
	return 0;
}

static void transfer(int fd, uint8_t *tx, uint8_t *rx, size_t len)
{
	int ret;
	int out_fd;
	struct spi_ioc_transfer tr = {
		.tx_buf = (unsigned long)tx,
		.rx_buf = (unsigned long)rx,
		.len = len,
		.delay_usecs = 0,
		.speed_hz = 500000,
		.bits_per_word = 8,
	};

	ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
	if (ret < 1)
	{
		printf("can't send spi message");
		exit(-1);
	}
}

static void chip_id(int fd)
{
	unsigned char val[2];
	
	transfer(fd, &mpu6500_regs[0], val, 2);
	printf("read id = 0x%x\n", val[1]);

	return;
}

static void acc_read(int fd)
{
	struct acc_measure acc;
	unsigned char val[2];

	acc.x = 0;
	acc.y = 0;
	acc.z = 0;

	// accelerometer measurements
	transfer(fd, &mpu6500_regs[2], val, 2);		// xout_h
	acc.x = (val[1] << 8);
	transfer(fd, &mpu6500_regs[4], val, 2);		// xout_l
	acc.x |= val[1];

	transfer(fd, &mpu6500_regs[6], val, 2);		// yout_h
	acc.y = (val[1] << 8);
	transfer(fd, &mpu6500_regs[8], val, 2);		// yout_l
	acc.y |= val[1];

	transfer(fd, &mpu6500_regs[10], val, 2);	// zout_h
	acc.z = (val[1] << 8);
	transfer(fd, &mpu6500_regs[12], val, 2);	// zout_l
	acc.z |= val[1];
	
	printf("accelerometer(x,y,z) 	= %.2f, %.2f, %.2f\n", 
		(float)(((float)(acc.x))/2048), 
		(float)(((float)(acc.y))/2048),	
		(float)(((float)(acc.z))/2048));

	return;
}

static void temp_read(int fd)
{
	signed short t = 0;
	unsigned char val[2];

	// temperature measurements
	transfer(fd, &mpu6500_regs[14], val, 2);		// tout_h
	t = (val[1] << 8);
	transfer(fd, &mpu6500_regs[16], val, 2);		// tout_l
	t |= val[1];
	
	printf("temprature(t) 		= %.2f °C\n", (((float)t/333.87)+21));
	return;
}

static void gyro_read(int fd)
{
	struct gyro_measure gyro;
	unsigned char val[2];

	// gyroscope measurements
	transfer(fd, &mpu6500_regs[18], val, 2);		// xout_h
	gyro.x = (val[1] << 8);
	transfer(fd, &mpu6500_regs[20], val, 2);		// xout_l
	gyro.x |= val[1];

	transfer(fd, &mpu6500_regs[22], val, 2);		// yout_h
	gyro.y = (val[1] << 8);
	transfer(fd, &mpu6500_regs[24], val, 2);		// yout_l
	gyro.y |= val[1];

	transfer(fd, &mpu6500_regs[26], val, 2);		// zout_h
	gyro.z = (val[1] << 8);
	transfer(fd, &mpu6500_regs[28], val, 2);		// zout_l
	gyro.z |= val[1];
	
	printf("gyroscope(x,y,z) 	= %.2f, %.2f, %.2f\n", 
		(float)(((float)(gyro.x))/16.4), 
		(float)(((float)(gyro.y))/16.4),
		(float)(((float)(gyro.z))/16.4));
	return;
}

